import rclpy
from rclpy.node import Node
from gobang_interfaces.msg import Serial
from collections import deque
import serial
import struct

# 串口配置（根据实际设备修改）
SERIAL_PORT = '/dev/ttyS3'  # 串口号
BAUD_RATE = 9600      # 波特率 

# 协议常量
START_BYTES = b'\xFE\x01'  # 起始位
END_BYTES = b'\xEF\x10'    # 结束位
CMD_CALIBRATE = 0x01       # 指令位

# 缓存机制
PACKET_BUFFER_SIZE = 100  # 最大数据包长度（包括起始位、指令位、数据、校验和、结束位）
PACKET_BUFF = deque(maxlen=PACKET_BUFFER_SIZE)

def calculate_checksum(command:bytes, data:bytes)->int:
    """计算校验和：指令位 + 所有数据字节之和,保留低2字节"""
    total = command
    for byte in data:
        total += byte
    return total & 0xFFFF  # 取低16位
    
def parse_packet(packet:bytes) -> dict:
    """
    解析接收到的数据包。

    :param packet: 接收到的原始字节数据包
    :return: 如果解析成功，返回包含指令(int)、数据(bytes)的字典；否则返回 None
    """  
    # 解析数据部分
    # 结构：起始位(2) + 指令(1) + 数据 + 校验和(2) + 结束位(2)
    try:
        # 使用struct解包（大端模式）
        command = packet[2]
        data = packet[3:-2]
        received_checksum = struct.unpack('>H', packet[-4:-2])[0]
        
        # 计算校验和
        calculated_checksum = calculate_checksum(command, data)
        
        if received_checksum != calculated_checksum:
            print(f"错误：校验和不匹配（收到：{received_checksum:04X}，计算：{calculated_checksum:04X}）")
            return None
        
        return {
            'command': command,
            'data': data,
        }
    
    except Exception as e:
        print(f"解析错误：{str(e)}")
        return None
        
def create_response(command:bytes, data:bytes):
    """生成应答数据包"""
    # 校验和计算：指令位 + 状态字节
    checksum = calculate_checksum(command, data)
    
    # 组装数据包
    response = (
        START_BYTES +                # 起始位
        command +  # 指令位
        data +                       # 数据部分（可以是状态字节等）
        struct.pack('>H', checksum) + # 校验和
        END_BYTES                    # 结束位
    )
    return response

class SerialNode(Node):

    def __init__(self):
        super().__init__('serial_node')
        self.ser = serial.Serial(SERIAL_PORT, BAUD_RATE)
        self.get_logger().info(f"监听串口 {SERIAL_PORT}，波特率 {BAUD_RATE}...")
        self.timer = self.create_timer(0, self.timer_callback)

    def timer_callback(self):
        try:
            while rclpy.ok():
                received_bytes = self.ser.read(1)
                self.get_logger().info(f"收到一个字节：{received_bytes.hex(' ')}")
                PACKET_BUFF.append(received_bytes)

                # 检查起始位和结束位
                packet_bytes = b''.join(PACKET_BUFF)
                if packet_bytes[:2] != START_BYTES or packet_bytes[-2:] != END_BYTES:
                    continue
                self.get_logger().info(f"完整数据包：{packet_bytes.hex(' ')}")
                PACKET_BUFF.clear()  # 清空缓存

                # 解析数据包
                data = parse_packet(packet_bytes)
                if data:
                    self.get_logger().info(f"解析成功：指令={data['command']:02X}，数据=({data['data'].hex(' ')})")
                    
                    # 生成应答
                    response = create_response(data['command'], 0x00)
                    self.ser.write(response)
                    self.get_logger().info(f"发送应答：{response.hex(' ')}")
        except Exception as e:
            print(f"错误：{str(e)}")
        finally:
            # 确保在退出时关闭串口
            if self.ser.is_open:
                self.ser.close()
                print("串口已关闭")
        
def main(args=None):
    rclpy.init(args=args)
    serial_node = SerialNode()
    rclpy.spin(serial_node)
    serial_node.destroy_node()
    rclpy.shutdown()